/*
 * File:   FollowSimulator.cpp
 * Author: Mohan
 *
 * Created on 11 juillet 2012, 15:11
 */

#include "FollowSimulator.h"
#include <cstdlib>
#include <stdlib.h>
#include <cmath>

FollowSimulator::FollowSimulator() {
    reset();
}

FollowSimulator::~FollowSimulator() {
}

void FollowSimulator::reset() {
	FollowState::States r;
	r.follow1 = (rand()%2 == 0);
	r.posX = 0;
	r.posY = 0;
    m_state.setValue(r);
    terminated = false;
}

double FollowSimulator::control(const action_t& a) {

	FollowState::States s = m_state.getValue();

	//Move agent. Agent remains in center
	if (a.getValue() == action_t::North)
		s.posY--;
	else if (a.getValue() == action_t::East)
		s.posX--;
	else if (a.getValue() == action_t::South)
		s.posY++;
	else if (a.getValue() == action_t::West)
		s.posX++;

	int reward = reward_function(m_state, a);

	//Move person
	int r = rand() % 100;
	if(s.follow1)
	{
	  	if(r < 30)
	   	{ }
	   	else if(r < 70)
	   	{
	   		s.posY++;
	   	}
	   	else if(r < 90)
	   	{
	   		s.posX++;
	   	}
	   	else if(r < 95)
	   	{
	   		s.posY--;
	   	}
	   	else
	   	{
	   		s.posX--;
	   	}
	}
	else
	{
	  	if(r < 10)
	   	{ }
	   	else if(r < 15)
	   	{
	   		s.posY++;
	   	}
	   	else if(r < 95)
	   	{
	   		s.posX++;
	   	}
	   	else if(r < 98)
	   	{
	   		s.posY--;
	   	}
	  	else
	   	{
	   		s.posX--;
	   	}
	}

	// Manhattan distance of person from the agent
	int dist = abs(s.posX) + abs(s.posY);
	if(dist >= 3)
	{
		terminated = true;
		reward = -20;
	}

	m_state.setValue(s);

	return reward;
}

FollowSimulator::obs_t FollowSimulator::sense() const{
    obs_t z;

    int r = rand() % 5;
    if(r == 0)
    {
    	z.setValue(obs_t::Unseen);
    }
    else
    {
    	if(m_state.getValue().posX == 0 && m_state.getValue().posY == 0) // Agent at same spot
    	{
    		z.setValue(obs_t::Same);
    	}
    	else if(m_state.getValue().posX == 0 && m_state.getValue().posY > 0) // Agent directly north
    	{
    		z.setValue(obs_t::North);
    	}
    	else if(m_state.getValue().posX == 0 && m_state.getValue().posY < 0) // Agent directly south
    	{
    		z.setValue(obs_t::South);
    	}
    	else if(m_state.getValue().posX > 0 && m_state.getValue().posY == 0) // Agent directly east
    	{
    		z.setValue(obs_t::East);
    	}
    	else if(m_state.getValue().posX < 0 && m_state.getValue().posY == 0) // Agent directly west
    	{
    		z.setValue(obs_t::West);
    	}
    	else if(m_state.getValue().posX > 0 && m_state.getValue().posY > 0) //Agent at North East
    	{
    		if(r%2 == 0)
    		{
    			z.setValue(obs_t::North);
    		}
    		else
    		{
    			z.setValue(obs_t::East);
    		}
    	}
    	else if(m_state.getValue().posX < 0 && m_state.getValue().posY > 0) //Agent at North West
    	{
    		if(r%2 == 0)
    		{
    			z.setValue(obs_t::North);
    		}
    	    else
    	    {
    	    	z.setValue(obs_t::West);
    	    }
    	}
    	else if(m_state.getValue().posX > 0 && m_state.getValue().posY < 0) //Agent at South East
    	{
    	 	if(r%2 == 0)
    	 	{
    	 		z.setValue(obs_t::South);
    	 	}
    	 	else
    	 	{
    	    	z.setValue(obs_t::East);
    	    }
    	}
    	else if(m_state.getValue().posX < 0 && m_state.getValue().posY < 0) //Agent at South West
    	{
    		if(r%2 == 0)
    		{
    			z.setValue(obs_t::South);
    		}
    		else
    		{
    			z.setValue(obs_t::West);
    		}
    	}
    }

    if (terminated)
        z.setFinal(true);
}
